#include <iostream>
#include <string>
#include "C:/Library/CPP/Packages/OpenCV-MinGW-Build-OpenCV-4.5.2-x64/include/opencv2/opencv.hpp"
#include "C:/Library/CPP/Packages/OpenCV-MinGW-Build-OpenCV-4.5.2-x64/include/opencv2/core.hpp"
#include "C:/Library/CPP/Packages/OpenCV-MinGW-Build-OpenCV-4.5.2-x64/include/opencv2/imgproc.hpp"
#include "C:/Library/CPP/Packages/OpenCV-MinGW-Build-OpenCV-4.5.2-x64/include/opencv2/calib3d.hpp"
#include <iterator> 
#include <algorithm>  
using namespace cv;
using namespace std;

template <typename T>
std::ostream& operator<< (std::ostream& out, const std::vector<T>& v) {
	if (!v.empty()) {
		out << '[';
		std::copy(v.begin(), v.end(), std::ostream_iterator<T>(out, ", "));
		out << "\b\b]";
	}
	return out;
}
int num = 20;
//相机中组标定板的位姿，X,Y,Z,w,x,y,z
Mat_<double> CalPose = (cv::Mat_<double>(num, 7) <<
-0.0161432, 0.0103094, 0.488668, 0.146414, 0.697833, -0.689621, -0.126549,
-0.0114543, -0.0213073, 0.498003, 0.466555, 0.68554, -0.504783, 0.239908,
-0.0155825, -0.00682934, 0.488578, 0.318446, 0.716581, -0.617932, 0.0571217,
-0.0129726, 0.025702, 0.496506, 0.0358354, -0.631077, 0.712434, 0.304788,
-0.00673946, 0.0349412, 0.510428, 0.215087, -0.520109, 0.685664, 0.461614,
-0.021476, 0.0334694, 0.455768, 0.304774, -0.577979, 0.647497, 0.392174,
-0.0239152, 0.0243138, 0.441229, 0.132035, -0.668051, 0.698008, 0.221495,
-0.0249502, 0.00896527, 0.433097, 0.053725, 0.711476, -0.700024, -0.0296834,
-0.0244491, -0.00817447, 0.432844, 0.232756, 0.705146, -0.652537, 0.150962,
-0.0225169, -0.022671, 0.443084, 0.393595, 0.650685, -0.560575, 0.327794,
-0.0189586, -0.0238116, 0.386863, -0.313611, -0.60484, 0.606246, -0.410223,
-0.0185833, -0.00927488, 0.377644, 0.142242, 0.68201, -0.675723, 0.24089,
-0.0190031, 0.00793324, 0.377218, -0.0414189, 0.712143, -0.698651, 0.0549773,
-0.0200881, 0.0231707, 0.385043, -0.225131, 0.693245, -0.671618, -0.132877,
-0.0216332, 0.0322016, 0.399647, -0.388639, 0.625616, -0.598292, -0.315612,
-0.007257, 0.0315007, 0.346464, -0.46494, 0.662062, -0.539883, -0.232448,
-0.00181716, 0.0225829, 0.331877, -0.313273, 0.70569, -0.633878, -0.045393,
0.00143378, 0.0074815, 0.324931, -0.1379, 0.701042, -0.685178, 0.141611,
0.00174147, -0.00980573, 0.325925, -0.0492638, -0.646815, 0.687594, -0.326218,
-0.000992474, -0.0244406, 0.334834, -0.22781, -0.548188, 0.642055, -0.485138
	);
//机械臂末端的位姿，X,Y,Z,w,x,y,z

Mat_<double> ToolPose = (cv::Mat_<double>(num, 7) <<
8.87216219528e-06, 0.200032041509, 0.0168240691861, 0.182949824976, 0.683045202952, 0.683012456293, -0.182955177062,
-2.50422235746e-06, 0.20000226722, 0.0167904686533, 0.249999132349, 0.9330132271, 0.250000657874, -0.0669807654706,
-3.83977948958e-06, 0.200002871809, 0.016791166793, 0.224143380002, 0.836519810521, 0.482959097795, -0.129401938409,
-3.83980270478e-06, 0.200004081022, 0.0167909922764, 0.129406737622, 0.482949946524, 0.836525876265, -0.224137689266,
-2.99749901135e-06, 0.200004534491, 0.016790730484, 0.06698663743, 0.24998603617, 0.933018547806, -0.249992323035,
-3.63853434383e-05, 0.272364932641, -0.0131495426265, 0.0337806281222, 0.256542868845, 0.957683037165, -0.126046919579,
-4.88251963316e-06, 0.272372026114, -0.0131400351743, 0.0652399932862, 0.495727346971, 0.858620962117, -0.112996390006,
-4.47969059764e-06, 0.272366499192, -0.0131539540841, 0.0922751279047, 0.701064910956, 0.701055817797, -0.092271510936,
-4.0044197326e-06, 0.272373392215, -0.0131398307277, 0.112994418201, 0.858613791415, 0.495740449879, -0.0652382173288,
-4.0037329518e-06, 0.272324154824, -0.013204205682, 0.12614539258, 0.957656287282, 0.256592090424, -0.033797551584,
-5.21282977395e-06, 0.349989096176, -0.0234041788918, 2.03532710897e-05, 0.965928097756, 0.258810566875, -4.96172454646e-06,
-4.74439527899e-06, 0.3499903647, -0.0234029572178, 1.59895492651e-05, 0.866026507116, 0.499998088632, -8.9571946977e-06,
-5.49780028651e-06, 0.350000836664, -0.0234029574189, 6.17062158679e-07, 0.70709004039, 0.707123521586, -6.17071990705e-07,
-4.88689703388e-06, 0.34999821868, -0.0234029574029, 2.61800179219e-06, 0.49998941946, 0.86603151236, -4.53449384118e-06,
-5.35544276792e-06, 0.350002407463, -0.0234029574175, -2.22501454624e-07, 0.258800946507, 0.96593067561, 1.74727807344e-06,
-6.48715113804e-06, 0.427659130022, -0.0131553278593, -0.033791616213, 0.256594238946, 0.957661020151, 0.126106675861,
-6.48707187417e-06, 0.427654415916, -0.013168967332, -0.0652733044587, 0.49571876177, 0.858616055478, 0.113052086357,
-6.41336021885e-06, 0.427653200967, -0.0131705956585, -0.0923088772377, 0.701066110398, 0.701046081233, 0.0923026111415,
-7.08604536536e-06, 0.427652189448, -0.0131708667198, -0.113049828447, 0.858607499469, 0.495734720171, 0.0652685633335,
-6.33966547293e-06, 0.427653706721, -0.0131704601267, -0.126093828844, 0.957663133116, 0.256593836137, 0.0337827330761
	);
//R和T转RT矩阵
Mat R_T2RT(Mat& R, Mat& T)
{
	Mat RT;
	Mat_<double> R1 = (cv::Mat_<double>(4, 3) << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
		R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
		R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),
		0.0, 0.0, 0.0);
	cv::Mat_<double> T1 = (cv::Mat_<double>(4, 1) << T.at<double>(0, 0), T.at<double>(1, 0), T.at<double>(2, 0), 1.0);

	cv::hconcat(R1, T1, RT);//C=A+B閿熸枻鎷烽敓鏂ゆ嫹鎷奸敓鏂ゆ嫹
	return RT;
}

//RT转R和T矩阵
void RT2R_T(Mat& RT, Mat& R, Mat& T)
{
	cv::Rect R_rect(0, 0, 3, 3);
	cv::Rect T_rect(3, 0, 1, 3);
	R = RT(R_rect);
	T = RT(T_rect);
}

//判断是否为旋转矩阵
bool isRotationMatrix(const cv::Mat& R)
{
	cv::Mat tmp33 = R({ 0,0,3,3 });
	cv::Mat shouldBeIdentity;

	shouldBeIdentity = tmp33.t() * tmp33;

	cv::Mat I = cv::Mat::eye(3, 3, shouldBeIdentity.type());

	return  cv::norm(I, shouldBeIdentity) < 1e-6;
}

/** @brief 欧拉角 -> 3*3 的R
*	@param 	eulerAngle		角度值
*	@param 	seq				指定欧拉角xyz的排列顺序如："xyz" "zyx"
*/
cv::Mat eulerAngleToRotatedMatrix(const cv::Mat& eulerAngle, const std::string& seq)
{
	CV_Assert(eulerAngle.rows == 1 && eulerAngle.cols == 3);

	eulerAngle /= 180 / CV_PI;
	cv::Matx13d m(eulerAngle);
	auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);
	auto xs = std::sin(rx), xc = std::cos(rx);
	auto ys = std::sin(ry), yc = std::cos(ry);
	auto zs = std::sin(rz), zc = std::cos(rz);

	cv::Mat rotX = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, xc, -xs, 0, xs, xc);
	cv::Mat rotY = (cv::Mat_<double>(3, 3) << yc, 0, ys, 0, 1, 0, -ys, 0, yc);
	cv::Mat rotZ = (cv::Mat_<double>(3, 3) << zc, -zs, 0, zs, zc, 0, 0, 0, 1);

	cv::Mat rotMat;

	if (seq == "zyx")		rotMat = rotX * rotY * rotZ;
	else if (seq == "yzx")	rotMat = rotX * rotZ * rotY;
	else if (seq == "zxy")	rotMat = rotY * rotX * rotZ;
	else if (seq == "xzy")	rotMat = rotY * rotZ * rotX;
	else if (seq == "yxz")	rotMat = rotZ * rotX * rotY;
	else if (seq == "xyz")	rotMat = rotZ * rotY * rotX;
	else {
		cv::error(cv::Error::StsAssert, "Euler angle sequence string is wrong.",
			__FUNCTION__, __FILE__, __LINE__);
	}

	if (!isRotationMatrix(rotMat)) {
		cv::error(cv::Error::StsAssert, "Euler angle can not convert to rotated matrix",
			__FUNCTION__, __FILE__, __LINE__);
	}

	return rotMat;
	//cout << isRotationMatrix(rotMat) << endl;
}

/** @brief 四元数转旋转矩阵
*	@note  数据类型double； 四元数定义 q = w + x*i + y*j + z*k
*	@param q 四元数输入{w,x,y,z}向量
*	@return 返回旋转矩阵3*3
*/
cv::Mat quaternionToRotatedMatrix(const cv::Vec4d& q)
{
	double w = q[0], x = q[1], y = q[2], z = q[3];

	double x2 = x * x, y2 = y * y, z2 = z * z;
	double xy = x * y, xz = x * z, yz = y * z;
	double wx = w * x, wy = w * y, wz = w * z;

	cv::Matx33d res{
		1 - 2 * (y2 + z2),	2 * (xy - wz),		2 * (xz + wy),
		2 * (xy + wz),		1 - 2 * (x2 + z2),	2 * (yz - wx),
		2 * (xz - wy),		2 * (yz + wx),		1 - 2 * (x2 + y2),
	};
	return cv::Mat(res);
}

/** @brief ((四元数||欧拉角||旋转向量) && 转移向量) -> 4*4 的Rt
*	@param 	m				1*6 || 1*7的矩阵  -> 6  {x,y,z, rx,ry,rz}   7 {x,y,z, qw,qx,qy,qz}
*	@param 	useQuaternion	如果是1*7的矩阵，判断是否使用四元数计算旋转矩阵
*	@param 	seq				如果通过欧拉角计算旋转矩阵，需要指定欧拉角xyz的排列顺序如："xyz" "zyx" 为空表示旋转向量
*/
cv::Mat attitudeVectorToMatrix(cv::Mat m, bool useQuaternion, const std::string& seq)
{
	CV_Assert(m.total() == 6 || m.total() == 7);
	if (m.cols == 1)
		m = m.t();
	cv::Mat tmp = cv::Mat::eye(4, 4, CV_64FC1);
	//如果使用四元数转换成旋转矩阵则读取m矩阵的第四个成员，读4个数据
	if (useQuaternion)	// normalized vector, its norm should be 1.
	{
		cv::Vec4d quaternionVec = m({ 3, 0, 4, 1 });
		quaternionToRotatedMatrix(quaternionVec).copyTo(tmp({ 0, 0, 3, 3 }));
		// cout << norm(quaternionVec) << endl; 
	}
	else
	{
		cv::Mat rotVec;
		if (m.total() == 6)
			rotVec = m({ 3, 0, 3, 1 });		//6
		else
			rotVec = m({ 7, 0, 3, 1 });		//10

		//如果seq为空表示传入的是旋转向量，否则"xyz"的组合表示欧拉角
		if (0 == seq.compare(""))
			cv::Rodrigues(rotVec, tmp({ 0, 0, 3, 3 }));
		else
			eulerAngleToRotatedMatrix(rotVec, seq).copyTo(tmp({ 0, 0, 3, 3 }));
	}
	tmp({ 3, 0, 1, 3 }) = m({ 0, 0, 3, 1 }).t();
	//std::swap(m,tmp);
	return tmp;
}
int main()
{
	//定义手眼标定矩阵
	std::vector<Mat> R_base2gripper;//基座到夹爪的旋转矩阵
	std::vector<Mat> t_base2gripper;//基座到夹爪的平移向量
	std::vector<Mat> R_target2cam;//标定板到相机的旋转矩阵
	std::vector<Mat> t_target2cam;//标定板到相机的平移向量
	Mat R_cam2base = (Mat_<double>(3, 3));
	Mat t_cam2base = (Mat_<double>(3, 1));

	vector<Mat> images;
	size_t num_images = num;

	// 读取末端，标定板的姿态矩阵 4*4
	std::vector<cv::Mat> vecHb;//基座到夹爪的变换矩阵列表
	std::vector<cv::Mat> vecHc;//标定板到相机的变换矩阵列表
	cv::Mat Hcb;//定义相机camera到基座的位姿矩阵
	Mat tempR, tempT;

	for (size_t i = 0; i < num_images/2; i++)//计算标定板位姿
	{
		cv::Mat tmp = attitudeVectorToMatrix(CalPose.row(i), true, ""); //转移向量转旋转矩阵
		vecHc.push_back(tmp);
		RT2R_T(tmp, tempR, tempT);
		R_target2cam.push_back(tempR);
		t_target2cam.push_back(tempT);
		
		tmp = attitudeVectorToMatrix(CalPose.row(i+num_images/2), true, ""); //转移向量转旋转矩阵
		vecHc.push_back(tmp);
		RT2R_T(tmp, tempR, tempT);
		R_target2cam.push_back(tempR);
		t_target2cam.push_back(tempT);

	}
	//cout << R_target2cam << endl;
	for (size_t i = 0; i < num_images/2; i++)//计算机械臂位姿
	{
		cv::Mat tmp = attitudeVectorToMatrix(ToolPose.row(i), true, ""); //机械臂位姿为欧拉角-旋转矩阵
		//机械臂基座位姿为欧拉角-旋转矩阵
		//眼在手外添加下句
		tmp = tmp.inv();
		vecHb.push_back(tmp);
		RT2R_T(tmp, tempR, tempT);
		R_base2gripper.push_back(tempR);
		t_base2gripper.push_back(tempT);
	
		tmp = attitudeVectorToMatrix(ToolPose.row(i+num_images/2), true, ""); //机械臂位姿为欧拉角-旋转矩阵
		//机械臂基座位姿为欧拉角-旋转矩阵
		//眼在手外添加下句
		tmp = tmp.inv();
		vecHb.push_back(tmp);
		RT2R_T(tmp, tempR, tempT);
		R_base2gripper.push_back(tempR);
		t_base2gripper.push_back(tempT);

	}
	//cout << t_gripper2base[0] << " " << t_gripper2base[1] << " " << t_gripper2base[2] << " " << endl;
	//cout << t_gripper2base << endl;
	//手眼标定，CALIB_HAND_EYE_TSAI法为11ms，最快
	calibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, R_cam2base, t_cam2base, CALIB_HAND_EYE_TSAI);

	Hcb = R_T2RT(R_cam2base, t_cam2base);//矩阵合并

	std::cout <<  "Hcb 矩阵为： "  << std::endl;
	std::cout << Hcb << std::endl;
	cout << "是否为旋转矩阵：" << isRotationMatrix(Hcb) << std::endl << std::endl;//判断是否为旋转矩阵

	//Tool_In_Base*Hcg*Cal_In_Cam，用第一组和第二组进行对比验证
	cout << "第一组和第二组手眼数据验证："<< endl;
	cout << vecHb[0] * Hcb * vecHc[0] << endl << vecHb[1] * Hcb * vecHc[1] << endl << endl;//.inv()
	
	cout << "第一组和第二组手眼数据验证(arcuo2greper)："<< endl;
	cout << vecHb[0].inv() * Hcb * vecHc[0].inv() << endl \
		 << vecHb[1].inv() * Hcb * vecHc[1].inv() << endl << endl;

	cout << "标定板在相机中的位姿：" << endl;
	cout << vecHc[0] << endl;
	cout << "手眼系统反演的位姿为：" << endl;
	//用手眼系统预测第一组数据中标定板相对相机的位姿，是否与vecHc[1]相同
	cout << Hcb.inv() * vecHb[0].inv() * vecHb[1] * Hcb * vecHc[1] << endl << endl;

	cout << "----手眼系统测试----"  << endl;
	cout << "夹爪下标定板XYZ为："<< endl;
	for (int i = 0; i < vecHc.size(); ++i)
	{
		cv::Mat cheesePos{ 0.0,0.0,0.0,1.0 };//4*1矩阵，单独求夹爪下，标定板的xyz
		cv::Mat worldPos = vecHb[i] * Hcb * vecHc[i] * cheesePos;
		cout << i << ": " << worldPos.t() << endl;
	}
	getchar();
}
